#include<Algorithm/poisson_construct_base.h>
#include<tool/libcell_octree_iterator.h>



double fx_dot_fx[5]={1/120.0,13/60.0,11/20.0,13/60.0,1/120.0},fx_dot_Dfx[5]={-1/24.0,-5/12.0,0, 5/12.0,1/24.0}, fx_dot_DDfx[5]={1/6.0,1/3.0, -1,1/3.0,1/6.0};
//fx_dot_DDfxi=DDfx_dot_fxi
//fx_dot_Dfxi=-Dfx_dot_fxi

void poisson_grid_free(Poisson_Grid*pg)
{
    if(pg==NULL)
    {
        return;
    }
    if(pg->pgps!=NULL&&pg->size>0)
    {
        for(int i=0;i<pg->size;i++)
        {
            for(int j=0;j<pg->size;j++)
            {
                for(int k=0;k<pg->size;k++)
                {
                    SAFE_FREE(pg->pgps[i][j][k]);
                }
                free(pg->pgps[i][j]); 

            }
            free(pg->pgps[i]);
        }
        free(pg->pgps);
    }
    SAFE_FREE(pg->solve);
    free_node(pg->candidate);
    free(pg);

}

void poisson_grid_init_with_depth(Poisson_Grid*pg,int depth)
{
    pg->depth=depth;
    pg->size=pow(2,depth);
    pg->width=2*pg->range/(double)(pg->size);
    // double *pos_xs=(double*)malloc(sizeof(double)*pg->size);
    // memset(pos_xs,0,sizeof(double)*pg->size);
    // for(int i=0;i<pg->size;i++)
    // {
    //     pos_xs[i]=(i+0.5)*pg->width-pg->range;
    // }
    pg->pgps=(Poisson_Grid_Prop****)malloc(sizeof(Poisson_Grid_Prop***)*pg->size); 
    for(int i=0;i<pg->size;i++)
    {
        pg->pgps[i]=(Poisson_Grid_Prop***)malloc(sizeof(Poisson_Grid_Prop**)*pg->size);
        for(int j=0;j<pg->size;j++)
        {
            pg->pgps[i][j]=(Poisson_Grid_Prop**)malloc(sizeof(Poisson_Grid_Prop*)*pg->size);
            memset(pg->pgps[i][j],0,sizeof(Poisson_Grid_Prop*)*pg->size);
            // for(int k=0;k<pg->size;k++)
            // {
            //     poisson_grid_prop_init(&(pg->pgps[i][j][k])); 
            //     pg->pgps[i][j][k].pos[0]=pos_xs[i];
            //     pg->pgps[i][j][k].pos[1]=pos_xs[j];
            //     pg->pgps[i][j][k].pos[2]=pos_xs[k];
            // }
        }
    }
    // free(pos_xs);
}
void poisson_grid_init_with_accuracy(Poisson_Grid*pg,int fen)
{
    //pg->depth=depth;
    pg->size=fen;
    pg->width=2*pg->range/(double)(pg->size);
    // double *pos_xs=(double*)malloc(sizeof(double)*pg->size);

    // memset(pos_xs,0,sizeof(double)*pg->size);
    // for(int i=0;i<pg->size;i++)
    // {
    //     pos_xs[i]=(i+0.5)*pg->width-pg->range;
    // }
    pg->pgps=(Poisson_Grid_Prop****)malloc(sizeof(Poisson_Grid_Prop***)*pg->size); 
    for(int i=0;i<pg->size;i++)
    {
        pg->pgps[i]=(Poisson_Grid_Prop***)malloc(sizeof(Poisson_Grid_Prop**)*pg->size);
        for(int j=0;j<pg->size;j++)
        {
            pg->pgps[i][j]=(Poisson_Grid_Prop**)malloc(sizeof(Poisson_Grid_Prop*)*pg->size);
            memset(pg->pgps[i][j],0,sizeof(Poisson_Grid_Prop*)*pg->size);
            // for(int k=0;k<pg->size;k++)
            // {
            //     poisson_grid_prop_init(&(pg->pgps[i][j][k])); 
            //     pg->pgps[i][j][k].pos[0]=pos_xs[i];
            //     pg->pgps[i][j][k].pos[1]=pos_xs[j];
            //     pg->pgps[i][j][k].pos[2]=pos_xs[k];
            // }
        }
    }
    // free(pos_xs);
}


double* poisson_compute_b(Poisson_Grid*pg )
{

    int size=node_size(pg->candidate);
    double *re=(double*)malloc(sizeof(double)*size);
    memset(re,0,sizeof(double)*size);
    int sum=0,id=0,i=0,j=0,k=0;
    for(Node* nit=pg->candidate;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Poisson_Grid_Prop*pgp=(Poisson_Grid_Prop*)(nit->value);  
        id= pgp->id;
        i=pgp->indexs[0];j=pgp->indexs[1];k=pgp->indexs[2];

        for(int l=0;l<5;l++ )
        {
            for(int m=0;m<5;m++)
            {
                for(int n=0;n<5;n++)
                {
                    if(i+l-2<0||i+l-2>=pg->size||j+m-2<0||j+m-2>=pg->size||k+n-2<0||k+n-2>=pg->size)
                    {
                        continue;
                    }
                    if(pg->pgps[i+l-2][j+m-2][k+n-2]==NULL)
                    {
                        continue;
                    }
                    re[id]+= pg->pgps[i+l-2][j+m-2][k+n-2]->normal_coeff[0]*fx_dot_Dfx[l]*fx_dot_fx[m]*fx_dot_fx[n];
                    re[id]+= pg->pgps[i+l-2][j+m-2][k+n-2]->normal_coeff[1]*fx_dot_fx[l]*fx_dot_Dfx[m]*fx_dot_fx[n];
                    re[id]+= pg->pgps[i+l-2][j+m-2][k+n-2]->normal_coeff[2]*fx_dot_fx[l]*fx_dot_fx[m]*fx_dot_Dfx[n]; 
                
                }
            }
        }
        re[id]/=pg->width; 
    }
   
    //printf("sum:%d\n",sum);
    return re; 
}

double end_poisson_space_fun(double* p,void * tool)
{
    double re=0;
    Poisson_Grid* pg=(Poisson_Grid*)(tool);
    int i=(int)((p[0]+pg->range)/pg->width);
    int j=(int)((p[1]+pg->range)/pg->width);
    int k=(int)((p[2]+pg->range)/pg->width);
    int id=0;
    for(int l=0;l<3;l++)
    {
        for(int m=0;m<3;m++)
        {
            for(int n=0;n<3;n++)
            {
                if(i+l-1<0||i+l-1>=pg->size||j+m-1<0||j+m-1>=pg->size||k+n-1<0||k+n-1>=pg->size)
                {
                    continue;
                }
                if(pg->pgps[i+l-1][j+m-1][k+n-1]==NULL)
                {
                    continue;
                }
                id=pg->pgps[i+l-1][j+m-1][k+n-1]->id;
                //id=threeD_index2_id(i+l-1,j+m-1,k+n-1,pg->size);
                re+=(poisson_FF_deformation(pg->pgps[i+l-1][j+m-1][k+n-1]->pos,pg->width,p)*pg->solve[id]); 
            }
        } 
    }  

    return re;
}
double compute_average_poisson_iso(Poisson_Grid* pg)
{
    double test_iso=0;int sum=0,i=0,j=0,k=0,id=0;
    for(Node* nit=pg->candidate;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Poisson_Grid_Prop*pgp=(Poisson_Grid_Prop*)(nit->value);  
        id= pgp->id;
        i=pgp->indexs[0];j=pgp->indexs[1];k=pgp->indexs[2];
        if(pg->pgps[i][j][k]->normal_coeff[0]==0&&pg->pgps[i][j][k]->normal_coeff[1]==0&&pg->pgps[i][j][k]->normal_coeff[2]==0) 
        {
            continue; 
        }
        test_iso+=end_poisson_space_fun(pg->pgps[i][j][k]->pos,pg);
        sum++;

    }
   
    return test_iso/((double)sum);
}
static inline void fill_normal_coeff1(Poisson_Grid*pg,Poisson_Grid_Prop*pgp )
{
    int i=pgp->indexs[0],j=pgp->indexs[1],k=pgp->indexs[2];
    int num=0;
    for(int l=0;l<3;l++)
    {
        for(int m=0;m<3;m++)
        {
            for(int n=0;n<3;n++)
            {
                if(i+l-1<0||i+l-1>=pg->size||j+m-1<0||j+m-1>=pg->size||k+n-1<0||k+n-1>=pg->size )
                {
                    continue;
                }
                if(pg->pgps[i+l-1][j+m-1][k+n-1]==NULL)
                {
                    continue;
                }
                pgp->normal_coeff[0]+=pg->pgps[i+l-1][j+m-1][k+n-1]->normal_coeff[0];
                pgp->normal_coeff[1]+=pg->pgps[i+l-1][j+m-1][k+n-1]->normal_coeff[1];
                pgp->normal_coeff[2]+=pg->pgps[i+l-1][j+m-1][k+n-1]->normal_coeff[2];
                num++;
            }
        }
    }
    if(num>0)
    {
        normalize(pgp->normal_coeff,3);
    }

}

void fill_normal_coeff(Poisson_Grid*pg,Node* node_vertices)
{

    int i=0,j=0,k=0,id=0;
    double* normal=NULL;
    for(Node* nit=node_vertices;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        i=(int)((v->point[0]+pg->range)/pg->width);
        j=(int)((v->point[1]+pg->range)/pg->width);
        k=(int)((v->point[2]+pg->range)/pg->width);
        i=(i<0?0:(i>=pg->size? (pg->size-1): i ));
        j=(j<0?0:(j>=pg->size? (pg->size-1): j )); 
        k=(k<0?0:(k>=pg->size? (pg->size-1): k ));
        if(pg->pgps[i][j][k]!=NULL)
        {
            continue;
        }
        Poisson_Grid_Prop*pgp=(Poisson_Grid_Prop*)malloc(sizeof(Poisson_Grid_Prop));
        pg->pgps[i][j][k]=pgp;
        poisson_grid_prop_init(pgp);
        pgp->id=id;
        pgp->pos[0]=(i+0.5)*pg->width-pg->range;
        pgp->pos[1]=(j+0.5)*pg->width-pg->range;
        pgp->pos[2]=(k+0.5)*pg->width-pg->range;
        pgp->indexs[0]=i;pgp->indexs[1]=j;pgp->indexs[2]=k;
        pg->candidate=node_overlying(pg->candidate,pgp);
        
        normal=(double*)(v->prop);
        pgp->normal_coeff[0]=normal[0];
        pgp->normal_coeff[1]=normal[1];
        pgp->normal_coeff[2]=normal[2];
        
        id++;
    }

    for(Node* nit=pg->candidate;nit!=NULL;nit=(Node*)(nit->Next))
    {
        Poisson_Grid_Prop*pgp=(Poisson_Grid_Prop*)(nit->value);
        int i=pgp->indexs[0],j=pgp->indexs[1],k=pgp->indexs[2];

        for(int l=0;l<5;l++)
        {
            for(int m=0;m<5;m++)
            {
                for(int n=0;n<5;n++)
                {
                    if(i+l-2<0||i+l-2>=pg->size||j+m-2<0||j+m-2>=pg->size||k+n-2<0||k+n-2>=pg->size )
                    {
                        continue;
                    }

                    if(pg->pgps[i+l-2][j+m-2][k+n-2]==NULL)
                    {

                        Poisson_Grid_Prop*pgp1=(Poisson_Grid_Prop*)malloc(sizeof(Poisson_Grid_Prop));
                        pg->pgps[i+l-2][j+m-2][k+n-2]=pgp1;
                        poisson_grid_prop_init(pgp1);
                        pgp1->id=id;
                        pgp1->pos[0]=(i+l-2+0.5)*pg->width-pg->range;
                        pgp1->pos[1]=(j+m-2+0.5)*pg->width-pg->range;
                        pgp1->pos[2]=(k+n-2+0.5)*pg->width-pg->range;
                        
                      

                        pgp1->indexs[0]=i+l-2;pgp1->indexs[1]=j+m-2;pgp1->indexs[2]=k+n-2;
                        
                        pg->candidate=node_overlying(pg->candidate,pgp1);
                        // fill_normal_coeff1(pg,pgp1);

                        //pgp->normal_coeff[0]=normal[0];
                        //pgp->normal_coeff[1]=normal[1];
                        //pgp->normal_coeff[2]=normal[2];

                        // normal=(double*)(v->prop);
                        // pgp1->normal_coeff[0]=normal[0];
                        // pgp1->normal_coeff[1]=normal[1];
                        // pgp1->normal_coeff[2]=normal[2];
                        
                        id++;
                    }    
                }
            }
        }
    }
}

static double* marching_cubes_compute_normal(double* p,double step,double ( * fun)(double* p,void* tool),void* tool1 )
{
    double* re=(double*)malloc(sizeof(double)*3);
    double temp_p[3]={0};
    double value=fun(p,tool1);
    for(int i=0;i<3;i++)
    {
        memmove(temp_p,p,sizeof(double)*3);
        temp_p[i]+=step;
        double value1=fun(temp_p,tool1);
        re[i]=(value1-value)/step;  
    }
    return re;
}


Node**** my_marching_cubes_generate_trianglesn(Node* node_v,double iso,int len_grids,double(*fun)(double* ,void*),
    void* tool,Node**tris1)
{

    Node**** re=(Node****)malloc(sizeof(Node***)*len_grids);

    GRIDCELL grid;  
    double step=2.0/(double)len_grids;
    for(int i=0;i<len_grids;i++)
    {
        re[i]=(Node***)malloc(sizeof(Node**)*len_grids);

        for(int j=0;j<len_grids;j++)
        {
            re[i][j]=(Node**)malloc(sizeof(Node*)*len_grids);
            memset(re[i][j],0,sizeof(Node*)*len_grids);
            
        }
    }

    OC_Node*ocn=(OC_Node*)malloc(sizeof(OC_Node));
    oc_node_init(ocn);
    ocn->value=node_copy(node_v);
    ocn->loc_min[0]=-1;  ocn->loc_min[1]=-1; ocn->loc_min[2]=-1;
    ocn->loc_max[0]=1;   ocn->loc_max[1]=1;  ocn->loc_max[2]=1;
    for(int i=0;i<7;i++)
    {
        oc_node_divide_all_leaves(ocn);
    } 

    for(auto it= octree_begin(ocn);it!=octree_end(ocn);it++)
    {
        if(it.it->value!=NULL )
        {
            int min_i=(int)floor((1+it.it->loc_min[0])/step);
            int min_j=(int)floor((1+it.it->loc_min[1])/step);
            int min_k=(int)floor((1+it.it->loc_min[2])/step);
            min_i<0?(min_i=0):(min_i=min_i);
            min_j<0?(min_j=0):(min_j=min_j);
            min_k<0?(min_k=0):(min_k=min_k);
 
 
            int max_i=(int)ceil((1+it.it->loc_max[0])/step);
            int max_j=(int)ceil((1+it.it->loc_max[1])/step);
            int max_k=(int)ceil((1+it.it->loc_max[2])/step);
            max_i>=len_grids?(max_i=len_grids-1):(max_i=max_i);
            max_j>=len_grids?(max_j=len_grids-1):(max_j=max_j);
            max_k>=len_grids?(max_k=len_grids-1):(max_k=max_k);
 

            for(int i=min_i;i<=max_i;i++)
            {
                for(int j=min_j;j<=max_j;j++)
                {
                    for(int k=min_k;k<=max_k;k++)
                    {
                        if(re[i][j][k]==NULL)
                        {
                            grid.p[0][0]=-1.0+i*step; grid.p[0][1]=-1.0+(j+1)*step; grid.p[0][2]=-1.0+k*step;       
                            grid.p[1][0]=-1.0+(i+1)*step; grid.p[1][1]=-1.0+(j+1)*step; grid.p[1][2]=-1.0+k*step;
                            grid.p[2][0]=-1.0+(i+1)*step; grid.p[2][1]=-1.0+(j)*step; grid.p[2][2]=-1.0+k*step;
                            grid.p[3][0]=-1.0+(i)*step; grid.p[3][1]=-1.0+(j)*step; grid.p[3][2]=-1.0+k*step;

                            grid.p[4][0]=-1.0+(i)*step; grid.p[4][1]=-1.0+(j+1)*step; grid.p[4][2]=-1.0+(k+1)*step;
                            grid.p[5][0]=-1.0+(i+1)*step; grid.p[5][1]=-1.0+(j+1)*step; grid.p[5][2]=-1.0+(k+1)*step;
                            grid.p[6][0]=-1.0+(i+1)*step; grid.p[6][1]=-1.0+(j)*step; grid.p[6][2]=-1.0+(k+1)*step;
                            grid.p[7][0]=-1.0+(i)*step; grid.p[7][1]=-1.0+(j)*step; grid.p[7][2]=-1.0+(k+1)*step;
                
                            for(int l=0;l<8;l++)
                            { 
                                grid.val[l]=fun(grid.p[l],tool);
                                double *normal=marching_cubes_compute_normal(grid.p[l],step,fun,tool);    
                                memmove(grid.normals[l],normal,sizeof(double)*3);
                                free(normal);
                            }
                            TRIANGLE triangles[5];
                            int len=polygonise(grid,iso,triangles);
                            for(int l=0;l<len;l++)
                            {
                                TRIANGLE*triangle=(TRIANGLE*)malloc(sizeof(TRIANGLE));
                                memmove(triangle,&triangles[l],sizeof(TRIANGLE));  
                                triangle->i=i; triangle->j=j;triangle->k=k;triangle->prop=NULL;
                                re[i][j][k]=node_overlying(re[i][j][k],triangle); 
                                (*tris1)=node_overlying( (*tris1),triangle);
                            }
                        }
                    }
                }
            } 
        }
    }    

    oc_node_free(ocn); 
 
    
    // for(Node* nit=node_v;nit!=NULL;nit=(Node*)(nit->Next))
    // {
    //     template_v*v=(template_v*)(nit->value);
    //     int i1=(int)round((v->point[0]+1)/step);
    //     int j1=(int)round((v->point[1]+1)/step);
    //     int k1=(int)round((v->point[2]+1)/step);
    //     for(int i=i1-2;i<=i1+2;i++)
    //     {
    //         for(int j=j1-2;j<=j1+2;j++)
    //         {
    //             for(int k=k1-2;k<=k1+2;k++)
    //             {
    //                 if(i>=0&&j>=0&&k>=0&&i<len_grids&&j<len_grids&&k<len_grids&&re[i][j][k]==NULL)
    //                 {
    //                     grid.p[0][0]=-1.0+i*step; grid.p[0][1]=-1.0+(j+1)*step; grid.p[0][2]=-1.0+k*step;       
    //                     grid.p[1][0]=-1.0+(i+1)*step; grid.p[1][1]=-1.0+(j+1)*step; grid.p[1][2]=-1.0+k*step;
    //                     grid.p[2][0]=-1.0+(i+1)*step; grid.p[2][1]=-1.0+(j)*step; grid.p[2][2]=-1.0+k*step;
    //                     grid.p[3][0]=-1.0+(i)*step; grid.p[3][1]=-1.0+(j)*step; grid.p[3][2]=-1.0+k*step;

    //                     grid.p[4][0]=-1.0+(i)*step; grid.p[4][1]=-1.0+(j+1)*step; grid.p[4][2]=-1.0+(k+1)*step;
    //                     grid.p[5][0]=-1.0+(i+1)*step; grid.p[5][1]=-1.0+(j+1)*step; grid.p[5][2]=-1.0+(k+1)*step;
    //                     grid.p[6][0]=-1.0+(i+1)*step; grid.p[6][1]=-1.0+(j)*step; grid.p[6][2]=-1.0+(k+1)*step;
    //                     grid.p[7][0]=-1.0+(i)*step; grid.p[7][1]=-1.0+(j)*step; grid.p[7][2]=-1.0+(k+1)*step;
                
    //                     for(int l=0;l<8;l++)
    //                     { 
    //                         grid.val[l]=fun(grid.p[l],tool);
    //                         double *normal=marching_cubes_compute_normal(grid.p[l],step,fun,tool);    
    //                         memmove(grid.normals[l],normal,sizeof(double)*3);
    //                         free(normal);
    //                     }
    //                     TRIANGLE triangles[5];
    //                     int len=polygonise(grid,iso,triangles);
    //                     for(int l=0;l<len;l++)
    //                     {
    //                         TRIANGLE*triangle=(TRIANGLE*)malloc(sizeof(TRIANGLE));
    //                         memmove(triangle,&triangles[l],sizeof(TRIANGLE));  
    //                         triangle->i=i; triangle->j=j;triangle->k=k;triangle->prop=NULL;
    //                         re[i][j][k]=node_overlying(re[i][j][k],triangle); 
    //                         (*tris1)=node_overlying( (*tris1),triangle);
    //                     }
    //                 }
    //             }
    //         }
    //     }
    // }
    return re;
}


Node**** my_marching_cubes_generate_trianglesn1(Node* node_v,double iso,int len_grids,double(*fun)(double* ,void*),
    void* tool,Node**tris1)
{

    Node**** re=(Node****)malloc(sizeof(Node***)*len_grids);

    GRIDCELL grid;  
    double step=2.0/(double)len_grids;
    for(int i=0;i<len_grids;i++)
    {
        re[i]=(Node***)malloc(sizeof(Node**)*len_grids);

        for(int j=0;j<len_grids;j++)
        {
            re[i][j]=(Node**)malloc(sizeof(Node*)*len_grids);
            memset(re[i][j],0,sizeof(Node*)*len_grids);
            
        }
    }
    
    for(Node* nit=node_v;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v*v=(template_v*)(nit->value);
        int i1=(int)round((v->point[0]+1)/step);
        int j1=(int)round((v->point[1]+1)/step);
        int k1=(int)round((v->point[2]+1)/step);
        for(int i=i1-2;i<=i1+2;i++)
        {
            for(int j=j1-2;j<=j1+2;j++)
            {
                for(int k=k1-2;k<=k1+2;k++)
                {
                    if(i>=0&&j>=0&&k>=0&&i<len_grids&&j<len_grids&&k<len_grids&&re[i][j][k]==NULL)
                    {
                        grid.p[0][0]=-1.0+i*step; grid.p[0][1]=-1.0+(j+1)*step; grid.p[0][2]=-1.0+k*step;       
                        grid.p[1][0]=-1.0+(i+1)*step; grid.p[1][1]=-1.0+(j+1)*step; grid.p[1][2]=-1.0+k*step;
                        grid.p[2][0]=-1.0+(i+1)*step; grid.p[2][1]=-1.0+(j)*step; grid.p[2][2]=-1.0+k*step;
                        grid.p[3][0]=-1.0+(i)*step; grid.p[3][1]=-1.0+(j)*step; grid.p[3][2]=-1.0+k*step;

                        grid.p[4][0]=-1.0+(i)*step; grid.p[4][1]=-1.0+(j+1)*step; grid.p[4][2]=-1.0+(k+1)*step;
                        grid.p[5][0]=-1.0+(i+1)*step; grid.p[5][1]=-1.0+(j+1)*step; grid.p[5][2]=-1.0+(k+1)*step;
                        grid.p[6][0]=-1.0+(i+1)*step; grid.p[6][1]=-1.0+(j)*step; grid.p[6][2]=-1.0+(k+1)*step;
                        grid.p[7][0]=-1.0+(i)*step; grid.p[7][1]=-1.0+(j)*step; grid.p[7][2]=-1.0+(k+1)*step;
                
                        for(int l=0;l<8;l++)
                        { 
                            grid.val[l]=fun(grid.p[l],tool);
                            double *normal=marching_cubes_compute_normal(grid.p[l],step,fun,tool);    
                            memmove(grid.normals[l],normal,sizeof(double)*3);
                            free(normal);
                        }
                        TRIANGLE triangles[5];
                        int len=polygonise(grid,iso,triangles);
                        for(int l=0;l<len;l++)
                        {
                            TRIANGLE*triangle=(TRIANGLE*)malloc(sizeof(TRIANGLE));
                            memmove(triangle,&triangles[l],sizeof(TRIANGLE));  
                            triangle->i=i; triangle->j=j;triangle->k=k;triangle->prop=NULL;
                            re[i][j][k]=node_overlying(re[i][j][k],triangle); 
                            (*tris1)=node_overlying( (*tris1),triangle);
                        }
                    }
                }
            }
        }
    }
    return re;
}


// static void  poisson_test_sparse_matrix(Poisson_Grid*pg )
// {
//     int len=pg->size*pg->size*pg->size;
//     SparseMatrix<double> A(len,len);
//     A.setZero();
//     A.reserve(VectorXi::Constant(pg->size*pg->size*pg->size,140));
//     int id=0,id1=0;
//     int sum=0;double temp=0;
//     for(int i=0;i<pg->size;i++)
//     {
//         for(int j=0;j<pg->size;j++)
//         {
//             for(int k=0;k<pg->size;k++)
//             {
//                 id=threeD_index2_id(i,j,k,pg->size);

//                 sum++;
//                 for(int l=0;l<5;l++ )
//                 {
//                     for(int m=0;m<5;m++)
//                     {
//                         for(int n=0;n<5;n++)
//                         {
//                             if(i+l-2<0||i+l-2>=pg->size||j+m-2<0||j+m-2>=pg->size||k+n-2<0||k+n-2>=pg->size)
//                             {
//                                 continue;
//                             }
//                             id1=threeD_index2_id(i+l-2,j+m-2,k+n-2,pg->size);

//                             temp=0;
//                             temp+=(fx_dot_DDfx[l]*fx_dot_fx[m]*fx_dot_fx[n]); 
//                             temp+=(fx_dot_fx[l]*fx_dot_DDfx[m]*fx_dot_fx[n]); 
//                             temp+=(fx_dot_fx[l]*fx_dot_fx[m]*fx_dot_DDfx[n]);
//                             temp/=(pg->width*pg->width);
//                             A.coeffRef(id,id1)=temp;    
                              
//                         }
//                     }
//                 }
//             }
//         }
//     }
//     A.makeCompressed();
//     ConjugateGradient<SparseMatrix<double>,Eigen::Upper> solver;
//     double*bi= poisson_compute_b(pg );
//     Eigen::VectorXd b(len);
//     for(int i=0;i<len;i++)
//     {
//         b.coeffRef(i)=bi[i];
//     }   
//     printf("end b\n"); 
//     free(bi);
//     VectorXd x= solver.compute(A).solve(b); 
//     pg->solve=(double*)malloc(sizeof(double)*len); 
//     for(int i=0;i<len;i++)
//     {
//         pg->solve[i]=x.coeffRef(i);
//     }
//     printf(" 1sum:%d\n",sum);
// }
